#include "RCPWM.h"
#include "hardwareDef.h"

uint16_t pwmSort[chNum] = {};//pwm信号排序数组
uint8_t chSort[chNum] = {};//通道号排序数组
uint8_t nA, nB;//比较器A、B查询排序数组时的索引号

extern int RCPWM[chNum];//RCPWM的数据数组，所有飞控的输出数据直接放这个数组就可以了

void INIT_SERVO()
{//初始化servo信号
  //首先定义各端口输出
  #ifdef pwmPin1
    pinMode(pwmPin1, OUTPUT);
  #endif
  #ifdef pwmPin2
    pinMode(pwmPin2, OUTPUT);
  #endif
  #ifdef pwmPin3
    pinMode(pwmPin3, OUTPUT);
  #endif
  #ifdef pwmPin4
    pinMode(pwmPin4, OUTPUT);
  #endif
  #ifdef pwmPin5
    pinMode(pwmPin5, OUTPUT);
  #endif
  #ifdef pwmPin6
    pinMode(pwmPin6, OUTPUT);
  #endif
  #ifdef pwmPin7
    pinMode(pwmPin7, OUTPUT);
  #endif
  #ifdef pwmPin8
    pinMode(pwmPin8, OUTPUT);
  #endif
  #ifdef pwmPin9
    pinMode(pwmPin9, OUTPUT);
  #endif
  #ifdef pwmPin10
    pinMode(pwmPin10, OUTPUT);
  #endif
  /*其次把定时器1打开，定时器1采用的是
    普通模式，溢出上限是0xFFFF，8分频。
    不能通过修改上限来改变周期，但是可
    以超调定时器的计数器TCNT1，让定时
    器1跃迁到需要的起始点上，实现周期
    的缩短。
  */
  TCCR1A = 0;//普通模式，事件A、B端口不连
  TCCR1B = 0b10;//普通模式，8分频，溢出上限0xFFFF，PWM的分辨率为0.5μs
  TCCR1C = 0;
  TCNT1 = 25535;//超调TCNT1至25535，这样一周期为40000
  OCR1A = 25535 + 3000;//事件A默认在1500的位置上
  OCR1B = 25535 + 3000;//事件B默认在1500的位置上
  TIMSK1 = 0b111;//打开比较器A、B和溢出事件

  //用户自定义初始化动作：舵面自检
}

void PWM_SORT()
{//PWM排序
  for (int i = 0; i < chNum; i ++)
  {//首先将排序数组内所有数据刷新
    pwmSort[i] = RCPWM[i];
  }
  //通道号也刷新掉
  #ifdef pwmPin1
    chSort[0] = pwmPin1;
  #endif
  #ifdef pwmPin2
    chSort[1] = pwmPin2;
  #endif
  #ifdef pwmPin3
    chSort[2] = pwmPin3;
  #endif
  #ifdef pwmPin4
    chSort[3] = pwmPin4;
  #endif
  #ifdef pwmPin5
    chSort[4] = pwmPin5;
  #endif
  #ifdef pwmPin6
    chSort[5] = pwmPin6;
  #endif
  #ifdef pwmPin7
    chSort[6] = pwmPin7;
  #endif
  #ifdef pwmPin8
    chSort[7] = pwmPin8;
  #endif
  #ifdef pwmPin9
    chSort[8] = pwmPin9;
  #endif
  #ifdef pwmPin10
    chSort[9] = pwmPin10;
  #endif
  
  for (int i = 0; i < chNum - 1; i ++)
  {//冒泡排序法
    for (int j = i + 1; j < chNum; j ++)
    {
      if (pwmSort[i] > pwmSort[j])
      {
        uint16_t x = pwmSort[i];
        uint8_t y = chSort[i];
        pwmSort[i] = pwmSort[j];
        chSort[i] = chSort[j];
        pwmSort[j] = x;
        chSort[j] = y;
      }
    }
  }
}

ISR(TIMER1_OVF_vect)
{//溢出事件，可以看作为起始事件
  TCNT1 = 25535;//超调TCNT1达到50Hz频率需要的周期
  //打开所有通道高电平
  #ifdef pwmPin1
    digitalWrite(pwmPin1, HIGH);
  #endif
  #ifdef pwmPin2
    digitalWrite(pwmPin2, HIGH);
  #endif
  #ifdef pwmPin3
    digitalWrite(pwmPin3, HIGH);
  #endif
  #ifdef pwmPin4
    digitalWrite(pwmPin4, HIGH);
  #endif
  #ifdef pwmPin5
    digitalWrite(pwmPin5, HIGH);
  #endif
  #ifdef pwmPin6
    digitalWrite(pwmPin6, HIGH);
  #endif
  #ifdef pwmPin7
    digitalWrite(pwmPin7, HIGH);
  #endif
  #ifdef pwmPin8
    digitalWrite(pwmPin8, HIGH);
  #endif
  #ifdef pwmPin9
    digitalWrite(pwmPin9, HIGH);
  #endif
  #ifdef pwmPin10
    digitalWrite(pwmPin10, HIGH);
  #endif
  PWM_SORT();//排序
  OCR1A = pwmSort[0] * 2 + 25535;//比较器A设定目标
  OCR1B = pwmSort[1] * 2 + 25535;//比较器B设定目标
  nA = 0;//比较器A查询索引号复位
  nB = 1;//比较器B查询索引号复位
  TCNT2 = 127;
  TIMSK2 = 0b1;
}

ISR(TIMER1_COMPA_vect)
{//比较器A事件，查看的是0、2、4数组号
  while(nA < chNum)
  {//当索引号不超过通道数量时循环
    digitalWrite(chSort[nA], LOW);//该通道设置为低电平
    nA += 2;//比较器A查询索引号上涨
    if (nA >= chNum)
    {//如果索引号大于通道数量
      break;//跳出循环
    }
    if (abs(pwmSort[nA] - int(OCR1A - 25535) / 2) >= 3)
    {//如果下一次和这一次相隔大于10μs
      OCR1A = pwmSort[nA] * 2 + 25535;//比较器A设定到下一次需要的位置   
      break;//跳出循环
    }
  }
}

ISR(TIMER1_COMPB_vect)
{//比较器B事件，与A相同，查看的是1、3、5数组号
  while(nB < chNum)
  {
    digitalWrite(chSort[nB], LOW);
    nB += 2;
    if (nB >= chNum)
    {
      break;
    }
    if (abs(pwmSort[nB] - int(OCR1B - 25535) / 2) >= 3)
    {
      OCR1B = pwmSort[nB] * 2 + 25535;      
      break;
    }
  }
}